#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/float64.hpp"

class TestArm : public rclcpp::Node
{
public:
    TestArm() : Node("test_arm_node_cpp")
    {
        // 创建发布方
        joint1_pub = this->create_publisher<std_msgs::msg::Int32>("joint1_p_", 10);
        joint2_pub = this->create_publisher<std_msgs::msg::Float64>("joint2_p_", 10);
        joint3_pub = this->create_publisher<std_msgs::msg::Float64>("joint3_p_", 10);

        temp1_pub = this->create_publisher<std_msgs::msg::Float64>("temp1", 10);
        temp2_pub = this->create_publisher<std_msgs::msg::Float64>("temp2", 10);
        temp3_pub = this->create_publisher<std_msgs::msg::Float64>("temp3", 10);

        // 创建定时器，用于逐步更新关节数据
        timer = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&TestArm::update_joint_data, this));
    }

private:
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr joint1_pub;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr joint2_pub;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr joint3_pub;

    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr temp1_pub;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr temp2_pub;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr temp3_pub;

    rclcpp::TimerBase::SharedPtr timer;

    float target_j1 = 100.0;  // 固定目标值
    float target_j2 =  30.0;//目标值
    float target_j3 =  30.0;//目标值

    int current_j1 = 0;       // 当前值，逐步增加
    float current_j2 = 0.0;   // 当前值，逐步增加
    float current_j3 = 0.0;   // 当前值，逐步增加

    int step_j1 =  1;   // 步长
    float step_j2 = 1.1;      // 步长
    float step_j3 = 1.1;      // 步长

    void update_joint_data()
    {
        // 更新关节数据，逐步接近目标值
        if (current_j1 < target_j1)
            current_j1 += step_j1;
        if (current_j2 < target_j2)
            current_j2 += step_j2;
        if (current_j3 < target_j3)
            current_j3 += step_j3;

        // 发布关节数据
        auto msg_j1 = std_msgs::msg::Int32();
        auto msg_j2 = std_msgs::msg::Float64();
        auto msg_j3 = std_msgs::msg::Float64();

        msg_j1.data = current_j1;
        msg_j2.data = current_j2;
        msg_j3.data = current_j3;

        joint1_pub->publish(msg_j1);
        joint2_pub->publish(msg_j2);
        joint3_pub->publish(msg_j3);

        // 发布固定的目标值
        auto msg_temp1 = std_msgs::msg::Float64();
        auto msg_temp2 = std_msgs::msg::Float64();
        auto msg_temp3 = std_msgs::msg::Float64();

        msg_temp1.data = target_j1;
        msg_temp2.data = target_j2;
        msg_temp3.data = target_j3;

        temp1_pub->publish(msg_temp1);
        temp2_pub->publish(msg_temp2);
        temp3_pub->publish(msg_temp3);

        // 打印当前值
        RCLCPP_INFO(this->get_logger(), "当前关节数据: j1=%d, j2=%.2f, j3=%.2f", current_j1, current_j2, current_j3);
    }
};

int main(int argc, char const *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TestArm>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}